#include "ModbusSlave.hpp"

using namespace Modbus;

/* Slave Function. */

Slave::Function::Function(Slave *slave){
    this->slave = slave;
}

/* Slave. */

Slave::Slave(PortableStrategy *portable_strategy){
    for ( int n = 0; n < Slave::MAX_FUNCTION_COUNT; n++ ){
        this->function_list[n] = nullptr;
    }
    this->portable_strategy = portable_strategy;
}

int Slave::open(){
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->init() != true ){
        return Result::PORTABLE_STRATEGY_INIT_FAILED;
    }
    return Result::SUCCESS;
}

int Slave::close(){
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->deinit() != true ){
        return Result::PORTABLE_STRATEGY_DEINIT_FAILED;
    }
    return Result::SUCCESS;
}

int Slave::addFunction(Function *function){
    bool ret = false;
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( function == nullptr ){
        return Result::NO_FUNCTION;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    for ( int n = 0; n < MAX_FUNCTION_COUNT; n++ ){
        if ( this->function_list[n] == nullptr ){
            this->function_list[n] = function;
            ret = true;
            break;
        }
    }
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    if ( ret != true ){
        return Result::FUNCTION_LIST_FULL;
    }
    return Result::SUCCESS;
}

int Slave::removeFunction(Function *function){
    bool ret = false;
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( function == nullptr ){
        return Result::NO_FUNCTION;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    for ( int n = 0; n < Slave::MAX_FUNCTION_COUNT; n++ ){
        if ( this->function_list[n] == function ){
            this->function_list[n] = nullptr;
            ret = true;
            break;
        }
    }
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    if ( ret != true ){
        return Result::NO_FUNCTION;
    }
    return Result::SUCCESS;
}

int Slave::removeFunction(unsigned int function_code){
    bool ret = false;
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    for ( int n = 0; n < Slave::MAX_FUNCTION_COUNT; n++ ){
        if ( this->function_list[n] != nullptr ){
            if ( this->function_list[n]->getFunctionCode() == function_code ){
                this->function_list[n] = nullptr;
                ret = true;
                break;
            }
        }
    }
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    if ( ret != true ){
        return Result::NO_FUNCTION;
    }
    return Result::SUCCESS;
}
